/**
 * @file test_integration.cpp
 * @brief Integration tests for PX4 WorkQueue system
 * 
 * Tests multiple components working together:
 * - WorkQueueManager + WorkQueue + WorkItem
 * - ScheduledWorkItem with hrt_call
 * - WorkItemSingleShot with synchronization
 * - Multiple work queues with different priorities
 * - Concurrent operations and thread safety
 */

#include "px4_platform_common/px4_work_queue/work_queue_manager.hpp"
#include "px4_platform_common/px4_work_queue/work_item.hpp"
#include "px4_platform_common/px4_work_queue/scheduled_work_item.hpp"
#include "px4_platform_common/px4_work_queue/work_item_single_shot.hpp"
#include "px4_platform_common/hrt_call.h"

#include "FreeRTOS.h"
#include "task.h"

// Debug printf function
extern "C" {
	extern int debug_printf(const char *format, ...);
}

using namespace px4;

// Test data structures
struct IntegrationTestData
{
	uint32_t work_item_count{0};
	uint32_t scheduled_item_count{0};
	uint32_t single_shot_count{0};
	hrt_abstime last_execution_time{0};
	uint32_t sequence_number{0};
};

static IntegrationTestData test_data;

// Test WorkItem classes - use template to create unique types for each test
template<int N>
class TestWorkItem : public WorkItem
{
public:
	TestWorkItem() : WorkItem("test_work_item", px4::wq_configurations::test1) {}

protected:
	void Run() override
	{
		test_data.work_item_count++;
		test_data.last_execution_time = hrt_absolute_time();
	}
};

template<int N>
class TestScheduledItem : public ScheduledWorkItem
{
public:
	TestScheduledItem() : ScheduledWorkItem("test_scheduled", px4::wq_configurations::test1) {}

protected:
	void Run() override
	{
		test_data.scheduled_item_count++;
		test_data.last_execution_time = hrt_absolute_time();
	}
};

// Worker function for single-shot items
static void single_shot_worker(void *arg)
{
	IntegrationTestData *data = (IntegrationTestData *)arg;
	data->single_shot_count++;
	data->last_execution_time = hrt_absolute_time();
}

// Worker function for sequence testing
static void sequence_worker(void *arg)
{
	uint32_t *seq = (uint32_t *)arg;
	test_data.sequence_number = *seq;
}

extern "C" void test_integration(void)
{
	debug_printf("\n========================================\n");
	debug_printf("  Integration Tests\n");
	debug_printf("========================================\n");

	uint32_t total_tests = 0;
	uint32_t passed_tests = 0;

	// Start WorkQueueManager
	px4::WorkQueueManagerStart();
	vTaskDelay(pdMS_TO_TICKS(50));

	// Test 1: Multiple WorkItems on same queue
	{
		debug_printf("Test 1: Multiple WorkItems on same queue\n");

		static TestWorkItem<1> item1;
		static TestWorkItem<2> item2;
		static TestWorkItem<3> item3;

		test_data.work_item_count = 0;

		item1.ScheduleNow();
		item2.ScheduleNow();
		item3.ScheduleNow();

		vTaskDelay(pdMS_TO_TICKS(100));

		total_tests++;
		if (test_data.work_item_count == 3) {
			debug_printf("  ✓ All 3 items executed (count=%lu)\n", test_data.work_item_count);
			passed_tests++;
		} else {
			debug_printf("  ✗ Expected 3 executions, got %lu\n", test_data.work_item_count);
		}
	}

	// Test 2: WorkItem + ScheduledWorkItem interaction
	{
		debug_printf("Test 2: WorkItem + ScheduledWorkItem interaction\n");

		static TestWorkItem<4> work_item;
		static TestScheduledItem<1> scheduled_item;

		test_data.work_item_count = 0;
		test_data.scheduled_item_count = 0;

		// Schedule immediate work item
		work_item.ScheduleNow();

		// Schedule delayed item (50ms)
		scheduled_item.ScheduleDelayed(50000);

		// Wait 20ms - only work_item should have run
		vTaskDelay(pdMS_TO_TICKS(20));

		total_tests++;
		if (test_data.work_item_count == 1 && test_data.scheduled_item_count == 0) {
			debug_printf("  ✓ WorkItem ran, ScheduledItem delayed (wi=%lu, si=%lu)\n",
				test_data.work_item_count, test_data.scheduled_item_count);
			passed_tests++;
		} else {
			debug_printf("  ✗ Unexpected counts (wi=%lu, si=%lu)\n",
				test_data.work_item_count, test_data.scheduled_item_count);
		}

		// Wait another 40ms - both should have run
		vTaskDelay(pdMS_TO_TICKS(40));

		total_tests++;
		if (test_data.work_item_count == 1 && test_data.scheduled_item_count == 1) {
			debug_printf("  ✓ ScheduledItem executed after delay (wi=%lu, si=%lu)\n",
				test_data.work_item_count, test_data.scheduled_item_count);
			passed_tests++;
		} else {
			debug_printf("  ✗ Unexpected counts (wi=%lu, si=%lu)\n",
				test_data.work_item_count, test_data.scheduled_item_count);
		}
	}

	// Test 3: Periodic ScheduledWorkItem
	{
		debug_printf("Test 3: Periodic ScheduledWorkItem\n");

		static TestScheduledItem<4> periodic_item;

		test_data.scheduled_item_count = 0;

		// Schedule periodic execution every 20ms
		periodic_item.ScheduleOnInterval(20000);

		// Wait 110ms - should execute ~5-6 times
		vTaskDelay(pdMS_TO_TICKS(110));

		periodic_item.ScheduleClear();

		total_tests++;
		if (test_data.scheduled_item_count >= 4 && test_data.scheduled_item_count <= 7) {
			debug_printf("  ✓ Periodic execution count correct (%lu executions)\n",
				test_data.scheduled_item_count);
			passed_tests++;
		} else {
			debug_printf("  ✗ Expected 4-7 executions, got %lu\n",
				test_data.scheduled_item_count);
		}

		// Verify no more executions after clear
		uint32_t count_before = test_data.scheduled_item_count;
		vTaskDelay(pdMS_TO_TICKS(50));

		total_tests++;
		if (test_data.scheduled_item_count == count_before) {
			debug_printf("  ✓ No executions after ScheduleClear\n");
			passed_tests++;
		} else {
			debug_printf("  ✗ Unexpected execution after clear\n");
		}
	}

	// Test 4: WorkItemSingleShot synchronization
	{
		debug_printf("Test 4: WorkItemSingleShot synchronization\n");

		test_data.single_shot_count = 0;

		WorkItemSingleShot single_shot(px4::wq_configurations::test1, single_shot_worker, &test_data);

		hrt_abstime start_time = hrt_absolute_time();
		single_shot.ScheduleNow();
		single_shot.wait();  // Block until execution completes
		hrt_abstime elapsed = hrt_absolute_time() - start_time;

		total_tests++;
		if (test_data.single_shot_count == 1) {
			debug_printf("  ✓ Single-shot executed (count=%lu)\n", test_data.single_shot_count);
			passed_tests++;
		} else {
			debug_printf("  ✗ Expected 1 execution, got %lu\n", test_data.single_shot_count);
		}

		total_tests++;
		if (elapsed < 50000) {  // Should complete quickly
			debug_printf("  ✓ wait() blocked correctly (%llu us)\n", elapsed);
			passed_tests++;
		} else {
			debug_printf("  ✗ wait() took too long (%llu us)\n", elapsed);
		}
	}

	// Test 5: Multiple work queues (test1 and test2)
	{
		debug_printf("Test 5: Multiple work queues\n");

		class TestWorkItem1 : public WorkItem
		{
		public:
			TestWorkItem1() : WorkItem("item_wq1", px4::wq_configurations::test1) {}
		protected:
			void Run() override { test_data.work_item_count++; }
		};

		class TestWorkItem2 : public WorkItem
		{
		public:
			TestWorkItem2() : WorkItem("item_wq2", px4::wq_configurations::test2) {}
		protected:
			void Run() override { test_data.scheduled_item_count++; }
		};

		TestWorkItem1 item1;
		TestWorkItem2 item2;

		test_data.work_item_count = 0;
		test_data.scheduled_item_count = 0;

		item1.ScheduleNow();
		item2.ScheduleNow();

		vTaskDelay(pdMS_TO_TICKS(50));

		total_tests++;
		if (test_data.work_item_count == 1 && test_data.scheduled_item_count == 1) {
			debug_printf("  ✓ Items executed on different queues (wq1=%lu, wq2=%lu)\n",
				test_data.work_item_count, test_data.scheduled_item_count);
			passed_tests++;
		} else {
			debug_printf("  ✗ Unexpected counts (wq1=%lu, wq2=%lu)\n",
				test_data.work_item_count, test_data.scheduled_item_count);
		}
	}

	// Test 6: Sequential WorkItemSingleShot execution
	{
		debug_printf("Test 6: Sequential WorkItemSingleShot execution\n");

		uint32_t seq1 = 1, seq2 = 2, seq3 = 3;
		test_data.sequence_number = 0;

		WorkItemSingleShot shot1(px4::wq_configurations::test1, sequence_worker, &seq1);
		WorkItemSingleShot shot2(px4::wq_configurations::test1, sequence_worker, &seq2);
		WorkItemSingleShot shot3(px4::wq_configurations::test1, sequence_worker, &seq3);

		shot1.ScheduleNow();
		shot1.wait();

		total_tests++;
		if (test_data.sequence_number == 1) {
			debug_printf("  ✓ First single-shot executed (seq=%lu)\n", test_data.sequence_number);
			passed_tests++;
		} else {
			debug_printf("  ✗ Expected seq=1, got %lu\n", test_data.sequence_number);
		}

		shot2.ScheduleNow();
		shot2.wait();

		total_tests++;
		if (test_data.sequence_number == 2) {
			debug_printf("  ✓ Second single-shot executed (seq=%lu)\n", test_data.sequence_number);
			passed_tests++;
		} else {
			debug_printf("  ✗ Expected seq=2, got %lu\n", test_data.sequence_number);
		}

		shot3.ScheduleNow();
		shot3.wait();

		total_tests++;
		if (test_data.sequence_number == 3) {
			debug_printf("  ✓ Third single-shot executed (seq=%lu)\n", test_data.sequence_number);
			passed_tests++;
		} else {
			debug_printf("  ✗ Expected seq=3, got %lu\n", test_data.sequence_number);
		}
	}

	// Test 7: Mixed scheduling (immediate + delayed + periodic)
	{
		debug_printf("Test 7: Mixed scheduling (immediate + delayed + periodic)\n");

		static TestWorkItem<5> immediate_item;
		static TestScheduledItem<2> delayed_item;
		static TestScheduledItem<3> periodic_item;

		test_data.work_item_count = 0;
		test_data.scheduled_item_count = 0;

		// Schedule all three types
		immediate_item.ScheduleNow();           // Immediate
		delayed_item.ScheduleDelayed(40000);    // 40ms delay
		periodic_item.ScheduleOnInterval(25000); // 25ms period

		// Wait 50ms - immediate and 1-2 periodic should have run
		vTaskDelay(pdMS_TO_TICKS(60));

		total_tests++;
		if (test_data.work_item_count >= 1 && test_data.scheduled_item_count >= 1) {
			debug_printf("  ✓ Immediate and first periodic executed (wi=%lu, si=%lu)\n",
				test_data.work_item_count, test_data.scheduled_item_count);
			passed_tests++;
		} else {
			debug_printf("  ✗ Unexpected early counts (wi=%lu, si=%lu)\n",
				test_data.work_item_count, test_data.scheduled_item_count);
		}

		// Wait another 50ms - delayed should have run, periodic continues
		vTaskDelay(pdMS_TO_TICKS(50));

		total_tests++;
		if (test_data.scheduled_item_count >= 3) {  // delayed + 2-3 periodic
			debug_printf("  ✓ Delayed and periodic items executed (si=%lu)\n",
				test_data.scheduled_item_count);
			passed_tests++;
		} else {
			debug_printf("  ✗ Expected more executions (si=%lu)\n",
				test_data.scheduled_item_count);
		}

		periodic_item.ScheduleClear();
	}

	// Test 8: Rapid scheduling stress test
	{
		debug_printf("Test 8: Rapid scheduling stress test\n");

		static TestWorkItem<6> stress_item;

		test_data.work_item_count = 0;

		// Rapidly schedule 20 times
		for (int i = 0; i < 20; i++) {
			stress_item.ScheduleNow();
			vTaskDelay(pdMS_TO_TICKS(5));  // 5ms between schedules
		}

		vTaskDelay(pdMS_TO_TICKS(100));

		total_tests++;
		if (test_data.work_item_count == 20) {
			debug_printf("  ✓ All 20 rapid schedules executed (count=%lu)\n",
				test_data.work_item_count);
			passed_tests++;
		} else {
			debug_printf("  ✗ Expected 20 executions, got %lu\n",
				test_data.work_item_count);
		}
	}

	// Test 9: ChangeWorkQueue functionality
	{
		debug_printf("Test 9: ChangeWorkQueue functionality\n");

		class CountingWorkItem : public WorkItem
		{
		public:
			uint32_t count{0};
			CountingWorkItem() : WorkItem("counting", px4::wq_configurations::test1) {}
		protected:
			void Run() override { count++; }
		};

		CountingWorkItem item;

		// Run on test1
		item.ScheduleNow();
		vTaskDelay(pdMS_TO_TICKS(20));

		total_tests++;
		if (item.count == 1) {
			debug_printf("  ✓ Item ran on test1 (count=%lu)\n", item.count);
			passed_tests++;
		} else {
			debug_printf("  ✗ Expected count=1, got %lu\n", item.count);
		}

		// Change to test2
		item.ChangeWorkQueue(px4::wq_configurations::test2);
		item.ScheduleNow();
		vTaskDelay(pdMS_TO_TICKS(20));

		total_tests++;
		if (item.count == 2) {
			debug_printf("  ✓ Item ran on test2 after change (count=%lu)\n", item.count);
			passed_tests++;
		} else {
			debug_printf("  ✗ Expected count=2, got %lu\n", item.count);
		}
	}

	// Test 10: Concurrent single-shot items
	{
		debug_printf("Test 10: Concurrent single-shot items\n");

		static uint32_t concurrent_count = 0;
		auto concurrent_worker = [](void *arg) {
			concurrent_count++;
			vTaskDelay(pdMS_TO_TICKS(10));  // Simulate work
		};

		concurrent_count = 0;

		WorkItemSingleShot shot1(px4::wq_configurations::test1, concurrent_worker, nullptr);
		WorkItemSingleShot shot2(px4::wq_configurations::test2, concurrent_worker, nullptr);
		WorkItemSingleShot shot3(px4::wq_configurations::test1, concurrent_worker, nullptr);

		// Schedule all three
		shot1.ScheduleNow();
		shot2.ScheduleNow();
		shot3.ScheduleNow();

		// Wait for all
		shot1.wait();
		shot2.wait();
		shot3.wait();

		total_tests++;
		if (concurrent_count == 3) {
			debug_printf("  ✓ All 3 concurrent single-shots executed (count=%lu)\n",
				concurrent_count);
			passed_tests++;
		} else {
			debug_printf("  ✗ Expected 3 executions, got %lu\n", concurrent_count);
		}
	}

	// Print summary
	debug_printf("========================================\n");
	debug_printf("  Test Summary\n");
	debug_printf("========================================\n");
	debug_printf("Total tests: %lu\n", total_tests);
	debug_printf("Passed: %lu\n", passed_tests);
	debug_printf("Failed: %lu\n", total_tests - passed_tests);
	debug_printf("\n");

	if (passed_tests == total_tests) {
		debug_printf("✅ All Integration tests PASSED!\n");
	} else {
		debug_printf("❌ Some Integration tests FAILED!\n");
	}
	debug_printf("========================================\n\n");
}

